/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Robot;

import lejos.robotics.subsumption.Behavior;

/**
 *
 * @author zach
 */
public class orbitObstical implements Behavior{
    private boolean kill=false;
    public boolean takeControl() {
        return Constants.conditionCode==Constants.isObstical;
    }

    public void action() {
        kill=false;
        Configureation.getPilot().rotate(90f);
        float baring=Configureation.compassSensor().getDegreesCartesian();
        float radious=100f;
        while(!kill&&addwithAdjustment((int)baring,180,360)!=Configureation.compassSensor().getDegreesCartesian()){
            Configureation.getPilot().travelArc(25, radious);
            int zig=closestDistance();
            radious+=(100-zig);

        }
        Configureation.getPilot().rotate(-90f);
        Constants.conditionCode=0;

    }

    public void suppress() {
        kill=true;
    }

    private float addwithAdjustment(int i1,int i2,int max){
        if(i1+i2>max)
            return (i1+i2)%max;
        else
            return (i1+i2);
    }
    private int closestDistance(){
        int out=99999;
        float deg=5820.0f/90.0f;
        int tacho=0;
        Configureation.getSensorMotors()[0].rotate((int)(-15*deg));
        Configureation.getSensorMotors()[0].resetTachoCount();
        for(int i=0;i<30;i++){
            if(out>Configureation.ultraSoundSensor().getDistance()){
               out= Configureation.ultraSoundSensor().getDistance();
               tacho=Configureation.getSensorMotors()[0].getTachoCount();
            }
            Configureation.getSensorMotors()[0].rotate((int)(i*deg));
        }
        Configureation.getSensorMotors()[0].rotate(tacho-Configureation.getSensorMotors()[0].getTachoCount());
        return out;
    }
}
